#include "utility.hpp"

namespace LfUtils{


std::string fixed_precision(std::string str_fnum, int digits){
    std::string fixed_pre_fnum = str_fnum;
    auto pos = fixed_pre_fnum.find('.');
    
    if(pos != std::string::npos){
        digits = std::min(digits,static_cast<int>(str_fnum.size()-pos));
        fixed_pre_fnum = str_fnum.substr(0,pos+digits+1);
    }
    return fixed_pre_fnum;
}

void show_elasped_time_Info(std::vector<float>& vec_elasped_time){
    float tot_elasped_time = 0;
    float max_systime = 0;
    float min_systime = LLONG_MAX;
    for(auto& t: vec_elasped_time)
    {
        tot_elasped_time += t;
        if(t>max_systime)
            max_systime = t;
        if(t<min_systime)
            min_systime = t;
    }
    float avg_elasped_time = tot_elasped_time/vec_elasped_time.size();
    std::cout<<"================ Elasped Time Info ================"<<std::endl
             <<"elasped number "<<vec_elasped_time.size()<<"\n"
             <<"Total elasped_time: "<<tot_elasped_time/1000<<" s\n"
             <<"Avg elasped_time: "<<avg_elasped_time<<" "<<" ms, FPS: "<<1000./avg_elasped_time<<" Hz"<<std::endl
             <<"Max elasped_time: "<<max_systime<<" "<<" ms, FPS: "<<1000./max_systime<<" Hz"<<std::endl
             <<"Min elasped_time: "<<min_systime<<" "<<" ms, FPS: "<<1000./min_systime<<" Hz"<<std::endl;
}


void progress_info(int running_time, int start_time, int last_running_time){
    float progress = running_time - start_time;
    progress /= (last_running_time - start_time);

    if(progress<1){
        int barWidth = 50;
        std::cout << " Offline loading: [";
        int pos = barWidth * progress;
        for (int i = 0; i < barWidth; ++i) {
            if (i < pos) std::cout << "=";  
            else if (i == pos) std::cout << ">";
            else std::cout << " ";
        }
        std::cout << "] " << int(progress * 100.0) << " %\r%\r%\r";
        std::cout.flush();
    }
}


bool isRotationMatrix(const cv::Mat& R){
    cv::Mat Rt;
    cv::transpose(R, Rt);
    cv::Mat shouldBeIdentity = Rt * R;
    cv::Mat I = cv::Mat::eye(3,3, shouldBeIdentity.type());
    return  cv::norm(I, shouldBeIdentity) < 1e-6;
}


void Video_Info(const cv::VideoCapture& cap){
    if(!cap.isOpened())
        WARNING_MSG_EXIT("cap is not opened properly");
    std::cout<<"\n\n"
             <<"/***************************/\n"
             <<"/*        Video_Info       */\n"
             <<"/*"<<std::left<<std::setw(15)<<"FRAME_COUNT: "<<std::left<<std::setw(10)<<cap.get(cv::CAP_PROP_FRAME_COUNT)<<"*/\n"
             <<"/*"<<std::left<<std::setw(15)<<"FRAME_WIDTH: "<<std::left<<std::setw(10)<<cap.get(cv::CAP_PROP_FRAME_WIDTH)<<"*/\n"
             <<"/*"<<std::left<<std::setw(15)<<"FRAME_HEIGHT: "<<std::left<<std::setw(10)<<cap.get(cv::CAP_PROP_FRAME_HEIGHT)<<"*/\n"
             <<"/*"<<std::left<<std::setw(15)<<"FORMAT: "<<std::left<<std::setw(10)<<cap.get(cv::CAP_PROP_FORMAT)<<"*/\n"
             <<"/*"<<std::left<<std::setw(15)<<"FPS: "<<std::left<<std::setw(10)<<cap.get(cv::CAP_PROP_FPS)<<"*/\n"
             <<"/***************************/\n\n";
}

cv::Mat sudo_color(const cv::Mat& src, const cv::ColormapTypes& type){
    if(src.channels()!=1){
        ERROR_MSG_EXIT("src.channels()!=1");
    }
    cv::Mat norm_src, color_src;
    cv::normalize(src, norm_src, 0, 255, cv::NORM_MINMAX, CV_8UC1);
    cv::applyColorMap(norm_src, color_src, type);
    return color_src;
}

cv::Mat compute_gradient(cv::Mat src, cv::Mat& grad_mag, cv::Mat& grad_ori, float scale_factor, int ksize){
    cv::Mat gx, gy, gray_src, gray_blur;
    cv::cvtColor(src, gray_src, cv::COLOR_BGR2GRAY);
    // cv::GaussianBlur(gray_src, gray_blur, cv::Size(ksize, ksize), 0);
    if (scale_factor == 1) {
        gray_blur = gray_src;
    } else {
        cv::resize(gray_src,gray_blur,cv::Size(gray_src.cols*scale_factor, gray_src.rows*scale_factor));
    }
    cv::Sobel(gray_blur, gx, CV_64F,1,0,ksize);
    cv::Sobel(gray_blur, gy, CV_64F,0,1,ksize);
    cv::magnitude(gx,gy,grad_mag);
    cv::phase(gx,gy,grad_ori,true);
    
    cv::Mat vis_grad_ori(gray_blur.size(), CV_64FC1);
    // for(int y = 0; y<gray_blur.rows; y++){
    //     // auto ptr_gx = gx.ptr<double>(y);
    //     // auto ptr_gy = gy.ptr<double>(y);
    //     auto ptr_g_mag = grad_mag.ptr<double>(y);
    //     auto ptr_g_ori = grad_ori.ptr<double>(y);
    //     auto ptr_vis_grad_ori = vis_grad_ori.ptr<double>(y);
    //     for(int x = 0; x<gray_blur.cols; x++){
    //             // double pt_ori = atan2(ptr_gy[x],ptr_gx[x])*DegperRad;
    //             // if(pt_ori<0){
    //             //     pt_ori += 180;
    //             // }
    //             double pt_ori = ptr_g_ori[x];
    //             pt_ori += 90;
    //             if(pt_ori>=360)
    //                 pt_ori -= 360;
    //             if(pt_ori>180){
    //                 pt_ori -= 180;
    //             }
    //             ptr_vis_grad_ori[x] = pt_ori;
            
    //         if(ptr_g_mag[x]<20){
    //             ptr_vis_grad_ori[x] = 0;
    //         }
    //     }
    // }
    return vis_grad_ori;
};

void Point2fToxy(const std::vector<cv::Point2f> &pts, 
                 std::vector<float> &vec_x, std::vector<float> &vec_y) {
    vec_x.clear();  vec_x.reserve(pts.size());
    vec_y.clear();  vec_y.reserve(pts.size());

    for (auto &&pt: pts) {
        vec_x.push_back(pt.x); 
        vec_y.push_back(pt.y);
    }
}

void drawRotatedRect(cv::Mat &img, const cv::RotatedRect &rotatedRectangle, const cv::Scalar &color, int thickness){
    cv::Point2f vertices[4];
    rotatedRectangle.points(vertices);
    for (int i = 0; i < 4; ++i) {
        cv::line(img, vertices[i], vertices[(i+1)%4], color, thickness);
    }
}


cv::Mat get_Gaussian_kernel(int kernel_sz, float sigma){
    if(kernel_sz%2==0)
        ERROR_MSG_EXIT("Input kernel_sz is even");

    cv::Mat Gaussian_template = cv::Mat(kernel_sz, kernel_sz, CV_64FC1, double(0));
    int offset = (kernel_sz-1)/2;

    double const_factor =  1.0/(2*M_PI*sigma*sigma);
    for(int x=0; x<kernel_sz; x++){
        for(int y=0; y<kernel_sz; y++){
            /* G(x,y) = (2*pi*sigma^2)^-1 * exp( -(x^2+y^2)/(2*sigma^2) ) */
            double && gaussian_value = const_factor * std::exp(-(std::pow(x-offset,2)+std::pow(y-offset,2))/(2*sigma*sigma));
            Gaussian_template.at<double>(x,y) = gaussian_value;
            // std::cout<<"("<<x<<","<<y<<") = "<<Gaussian_template.at<double>(x,y)<<std::endl;
        }
    }
    return Gaussian_template;
}



} //namespace LfUtils

